#include "ble.h"
#include "string.h"
#include "breathing_led.h"
#include "pid.h"

uint8_t  uart1_buf[uart1_buf_size];//串口接收缓冲区
uint8_t  ble_txBuffer[64];//串口发送缓冲区

void ble_Init(void)
{

	if (HAL_UART_Receive_DMA(ble_uart, uart1_buf, uart1_buf_size) != HAL_OK)//手动打开第一次串口接收,并**同时**检查状态。第二次会返回busy。
  {
    // 错误处理
		HAL_UART_Receive_DMA(ble_uart,(uint8_t *) "uart1_error\n",12); 
    Error_Handler();
  }
}

void ble_print(uint8_t* buf,uint16_t len)
{
	HAL_UART_Transmit_DMA(ble_uart,(uint8_t *) buf, len);
}

//约定发送字符串格式如下
//"St+1000.0000E"
//"0123456789012"

void BLE_ParsePID_pos()
{	
/*#if pid_debug_pos_inc
	pid_inc *pid_t=&pid_speed;
	#else
	pid_pos *pid_t=&pid_angle;
#endif*/
	//上面的代码切换逻辑并没有生效,可能是被优化掉了(两个变量结构不一致感觉确实要出问题)
	pid_pos *pid_t=&pid_angle;
	
  uint8_t buf[uart1_buf_size];
	memcpy(buf,uart1_buf,uart1_buf_size);
	
  if(buf[0] != 'S' || buf[7] != '.' || buf[12] != 'E' ) 
			return;
	
	float val = 
							(buf[3] - '0') *1000 +
							(buf[4] - '0') *100+
							(buf[5] - '0') *10 +
							(buf[6] - '0') +
							(buf[8] - '0') *0.1f +
							(buf[9] - '0') *0.01f +
							(buf[10] - '0') *0.001f +
							(buf[11] - '0') *0.0001f;
	
	if(buf[2] == '+')
		val = val;
	else if(buf[2] == '-')
		val = -val;
	else
		return;
		
		if(val<-1000) 
			val=-1000;
		if(val>1000) 
			val=1000;

    switch(buf[1])
    {
        case 'p':
            pid_t->Kp = val;
            break;
        case 'i':
            pid_t->Ki = val;
            break;
        case 'd':
            pid_t->Kd = val;
            break;
        case 't':
            pid_t->target = val;
            break;
        default:
            break;
    }
}


#include <stdarg.h>
#include <string.h>

void vofa_send(int num, ...) {
    va_list args;
    va_start(args, num);
    
    // 拷贝所有float参数到缓冲区
    for (int i = 0; i < num; i++) {
        float value = va_arg(args, double); // float在可变参数中会自动提升为double
        memcpy(&ble_txBuffer[i * 4], &value, 4);
    }
    
    va_end(args);
    
    // 添加帧尾 0x00, 0x00, 0x80, 0x7F
    uint8_t tail = num * 4;
    ble_txBuffer[tail] = 0x00;
    ble_txBuffer[tail + 1] = 0x00;
    ble_txBuffer[tail + 2] = 0x80;
    ble_txBuffer[tail + 3] = 0x7F;

    ble_print((uint8_t*)ble_txBuffer, tail + 4);
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
   if ( huart->Instance== USART1)
   {
        if(uart1_buf[0] != 'S'  || uart1_buf[7] != '.' || uart1_buf[12] != 'E' ) 
        {
					memset(uart1_buf, 0, uart1_buf_size);
        }
				else{
					//业务逻辑
					BLE_ParsePID_pos();
					//uart1_buf[13]='\0';
				}
				//HAL_UART_Transmit(&huart1, uart1_buf, uart1_buf_size, 100); //串口发送字符串
				//memset(uart1_buf, 0, uart1_buf_size);
        //处理完数据后，必须重新开启下一次接收
        HAL_UART_Receive_DMA(&huart1, uart1_buf, uart1_buf_size);
   }
}

